/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    state_Bounce.cpp
  * @author  ClangWU
  * @brief   Finite State Machine for standing up.
  * @note    
  * @warning 
  *     - At least C++11 is required.														 
  ******************************************************************************
  */
 #include "state_Bounce.h"

 /* Private defines -----------------------------------------------------------*/
using namespace Quadruped;

/* Founctions ----------------------------------------------------------------*/
template <typename T>
FSM_Bounce<T>::FSM_Bounce(GaitScheduler<T> *p_gait,
                    StateEstimator<T> *p_estimator,
                    DesiredCoMState<T> *p_command,
                    Dynamic<T> *p_dynamic,
                    BalanceController<T> *p_controller,
                    T run_period):FSM_Base<T>::FSM_Base(run_period)
{
    FSM_Base<T>::_name = FSM_StateName::BOUNCE;
    p_BalanceController = p_controller;
    p_Dynamic = p_dynamic;
    p_GaitScheduler = p_gait;
    p_CoMCommand = p_command;
    p_StateEstimator = p_estimator;
}

template <typename T>
void FSM_Bounce<T>::RunState()
{
    for (int leg = 0; leg < 4; leg++)
    {
        //!>ready to bound
        if (p_GaitScheduler->GetEndBeforeJumpFlag()!=1)
        {
                pStandPos[leg] =    p_Dynamic->BodyState.rBody_M2B *
                                (Vec3<T>(0, 0,-p_CoMCommand->CoM_height) + p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation);   
                pBeforeBoundPos[leg]=p_Dynamic->BodyState.rBody_M2B *
                    (Vec3<T>(0, 0,-p_CoMCommand->CoM_beforeJumpHeight) + p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation);        
                p_GaitScheduler->SetFootPlacement(pStandPos[leg], pBeforeBoundPos[leg], 0.00, leg);  
        }
        //!>execute bound
        else if (p_GaitScheduler->GetEndBeforeJumpFlag()==1 && p_GaitScheduler->GetEndBoundJumpFlag()!=1)
        {
                pBoundJump[4]=p_Dynamic->BodyState.rBody_M2B *
                    (Vec3<T>(0, 0,-p_CoMCommand->CoM_boundJumpHeight) + p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation);        
                p_GaitScheduler->SetFootPlacement(pBeforeBoundPos[leg], pBoundJump[leg], 0.00, leg);  
        }
        //!>retract leg
        else if (p_GaitScheduler->GetEndBoundJumpFlag()==1 && p_GaitScheduler->GetEndRetractFlag()!=1)
        {
                pRetractPos[leg]=p_Dynamic->BodyState.rBody_M2B *
                    (Vec3<T>(0, 0,-p_CoMCommand->CoM_afterJumpHeight) + p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation);        
                p_GaitScheduler->SetFootPlacement(pRetractPos[leg], pRetractPos[leg], 0.00, leg);  
        }
        //!>fall down
        else if(p_GaitScheduler->GetEndRetractFlag()==1)
        {
                pFallPos[leg]=p_Dynamic->BodyState.rBody_M2B *
                    (Vec3<T>(0, 0,-p_CoMCommand->CoM_afterJumpHeight) + p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation);        
                p_GaitScheduler->SetFootPlacement(p_StateEstimator->p_foot_M[leg], pFallPos[leg], 0.00, leg); 
        }
    }

    //!< Generate trajectory in {M}.
    p_GaitScheduler->gaitGenerate();

    for (int leg = 0; leg < 4; leg++)
    {
        //!> set targets in {B}.
        if (p_GaitScheduler->GetEndRetractFlag()==1)
        {
            if ( p_StateEstimator->p_foot_M[leg](2)<=pFallPos[leg](2))
            {
                pDesiredFoot[leg] +=  0.1*(p_StateEstimator->p_foot_M[leg]-pFallPos[leg]);
            }
            else if (p_StateEstimator->p_foot_M[leg](2)>pFallPos[leg](2))
            {
                pDesiredFoot[leg] -=  0.1*(p_StateEstimator->p_foot_M[leg]-pFallPos[leg]);
            }
        }
        else
        {
            pDesiredFoot[leg] = p_GaitScheduler->GetFootPos(leg);
        }

        p_Dynamic->LegCommand[leg].p = (p_Dynamic->BodyState.rBody_M2B.transpose()
                                     * pDesiredFoot[leg]) - p_Dynamic->getHipLocation(leg);
        
        SysLog->Record(_INFO_,"Scheduler","Leg Command in M-Frame %d: %f, %f, %f",leg,
                        pDesiredFoot[leg](0),
                        pDesiredFoot[leg](1), 
                        pDesiredFoot[leg](2));
        SysLog->Record(_INFO_,"Scheduler","Leg Command in B-Frame %d: %f, %f, %f",leg,
                                    p_Dynamic->LegCommand[leg].p(0), p_Dynamic->LegCommand[leg].p(1), p_Dynamic->LegCommand[leg].p(2));
    }
}
/*!
*
*/

template <typename T>
int FSM_Bounce<T>::Transition(FSM_Base<T>* target_state)
{
    //Ready to switch
    switch (target_state->StateName())
    {
        case FSM_StateName::PASSIVE:
            p_GaitScheduler->changeGait(GaitType::STAND_UP);
            SysLog->Record(_INFO_,"FSM","State is switched to PASSIVE...");
            return 0;
        case FSM_StateName::LOCOMOTION:
            return 2;
        default:
            SysLog->Record(_ERROR_,"FSM","Error transition in STAND_UP, request is ignored...");
            return 2;
    }
}

template class FSM_Bounce<float>;
template class FSM_Bounce<double>;
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/